#include "MephistoCAM.h"

void allocate_ads5294_config( ADS5294_Config *cfg ){

    // cfg->reg_num        = _ADS5294_SPI_REG_NUM_ + _ADS5294_DEC_REG_NUM_; 
    cfg->reg_num        = _ADS5294_SPI_REG_NUM_ ;

    cfg->reg_flag       = (uint32_t*)malloc(sizeof(uint32_t)*cfg->reg_num);
    cfg->reg_addr       = (uint32_t*)malloc(sizeof(uint32_t)*cfg->reg_num);
    cfg->reg_data       = (uint32_t*)malloc(sizeof(uint32_t)*cfg->reg_num);
    cfg->reg_data_def   = (uint32_t*)malloc(sizeof(uint32_t)*cfg->reg_num);


    cfg->decimation_opt = -1;

//  初始化寄存器地址
    cfg->reg_addr[ 0]   = 0X00;
    cfg->reg_addr[ 1]   = 0X01;
    cfg->reg_addr[ 2]   = 0X02;
    cfg->reg_addr[ 3]   = 0X0A;
    cfg->reg_addr[ 4]   = 0X0F;
    cfg->reg_addr[ 5]   = 0X14;
    cfg->reg_addr[ 6]   = 0X1C;
    cfg->reg_addr[ 7]   = 0X23;
    cfg->reg_addr[ 8]   = 0X24;
    cfg->reg_addr[ 9]   = 0X25;
    cfg->reg_addr[10]   = 0X26;
    cfg->reg_addr[11]   = 0X27;
    cfg->reg_addr[12]   = 0X28;
    cfg->reg_addr[13]   = 0X29;
    cfg->reg_addr[14]   = 0X2A;
    cfg->reg_addr[15]   = 0X2B;
    cfg->reg_addr[16]   = 0X2C;
    cfg->reg_addr[17]   = 0X2D;
    cfg->reg_addr[18]   = 0X2E;
    cfg->reg_addr[19]   = 0X2F;
    cfg->reg_addr[20]   = 0X30;
    cfg->reg_addr[21]   = 0X31;
    cfg->reg_addr[22]   = 0X32;
    cfg->reg_addr[23]   = 0X33;
    cfg->reg_addr[24]   = 0X34;
    cfg->reg_addr[25]   = 0X35;
    cfg->reg_addr[26]   = 0X38;
    cfg->reg_addr[27]   = 0X42;
    cfg->reg_addr[28]   = 0X45;
    cfg->reg_addr[29]   = 0X46;
    cfg->reg_addr[30]   = 0X50;
    cfg->reg_addr[31]   = 0X51;
    cfg->reg_addr[32]   = 0X52;
    cfg->reg_addr[33]   = 0X53;
    cfg->reg_addr[34]   = 0X54;
    cfg->reg_addr[35]   = 0X55;
    cfg->reg_addr[36]   = 0XF0;

    // added @2023-10-27
    cfg->reg_addr[37]   = 0XD1;

    // int i, i_start = 37;

    // // decimation寄存器地址
    // for( i=0; i<96; i++ ){
    //     int idx = i_start + i;
    //     cfg->reg_addr[idx] = 0X5B + i;
    // }

    int i;
    for( i=0; i<cfg->reg_num; i++ ){
        cfg->reg_flag[i]    = 0;
        cfg->reg_data[i]    = 0;
        cfg->reg_data_def[i]= 0;
    }

}

void allocate_ads5294_config_adj( ADS5294_Config *cfg, int reg_num ){
    cfg->reg_num        = reg_num;
    cfg->reg_flag       = (uint32_t*)malloc(sizeof(uint32_t)*reg_num);
    cfg->reg_addr       = (uint32_t*)malloc(sizeof(uint32_t)*reg_num);
    cfg->reg_data       = (uint32_t*)malloc(sizeof(uint32_t)*reg_num);
    cfg->reg_data_def   = (uint32_t*)malloc(sizeof(uint32_t)*cfg->reg_num);
}

void free_ads5294_config( ADS5294_Config *ads5294_cfg ){
    free(ads5294_cfg->reg_flag);
    free(ads5294_cfg->reg_addr);
    free(ads5294_cfg->reg_data);
    free(ads5294_cfg->reg_data_def);
    free(ads5294_cfg);
}

//  initialize to default settings
int set_ads5294_config_default( ADS5294_Config *cfg, char *inifile, char *section ){

    dictionary *ini = iniparser_load(inifile);

    if( ini == NULL ){
        char ErrMsg[1024];
        sprintf(ErrMsg, "cannot parse file: %s\n", inifile);
        printDebugInfo(ErrMsg);
        exit(0);
    }

    char sec_param[64];
    char reg_use[64];

    sprintf(sec_param, "%s:decimation_opt", section);
    cfg->decimation_opt = iniparser_getint(ini, sec_param, -1);
    
    int i;
    for( i=0; i<cfg->reg_num; i++ ){
        
        sprintf(reg_use, "%s:use_0X%2X", section, cfg->reg_addr[i]);

        int opt = iniparser_getint( ini, reg_use, 0);
        if( opt != 1 ){
            // printf("> (ads5294) reg: 0X%02X is NOT to be used, pass ...\n", cfg->reg_addr[i]);
            cfg->reg_flag[i] = 0;
            continue;
        }

        sprintf(sec_param, "%s:0X%02X_def", section, cfg->reg_addr[i]);
        
        int tmp = iniparser_getint(ini, sec_param, -999);
        
        if( tmp < 0 ){
            char ErrMsg[1024];
            sprintf(ErrMsg, "unable to get default value for register: 0X%02X\n", cfg->reg_addr[i]);
            printDebugInfo(ErrMsg);
            exit(0);
        }

        cfg->reg_data_def[i]= (uint32_t)tmp;
        cfg->reg_data[i]    = cfg->reg_data_def[i];
    }

    return 0;
}

//  read ads5294 configuration data from *inifile
//  返回值为后续将要通过SPI进行配置的register的数目
int read_ads5294_config( ADS5294_Config *ads5294_cfg, char *inifile, char *section ){

    char regName[64], regFlag[64];
    int used_reg_num = 0;
    
    dictionary *ini;
	ini = iniparser_load(inifile);

	if( ini == NULL ){
	    char ErrMsg[1024];
	    sprintf(ErrMsg, "cannot parse file: %s\n", inifile);
        printDebugInfo(ErrMsg);
		exit(0);
	}   
	
    set_ads5294_config_default( ads5294_cfg, inifile, section );

    int idx;
    for( idx=0; idx<ads5294_cfg->reg_num; idx++ ){

        sprintf(regName, "%s:0X%02X", section, ads5294_cfg->reg_addr[idx]);
        sprintf(regFlag, "%s:use_0X%02X", section, ads5294_cfg->reg_addr[idx]);
        
        uint32_t use_reg  = iniparser_getint(ini, regFlag, 0);
        uint32_t reg_data = iniparser_getint(ini, regName, ads5294_cfg->reg_data[idx]);
        
        if( use_reg == 0 ){
#ifdef _DEBUG_ads5294_
            printf("> (ads5294) %s = %d, so %s will not be used\n", 
                    regFlag, use_reg, regName);
#endif
        } else if ( use_reg == 1 ){ // update reg_data
            ads5294_cfg->reg_flag[idx] = 1;
            ads5294_cfg->reg_data[idx] = reg_data;
            used_reg_num               += use_reg;
        } else {
            char errmsg[1024];
            sprintf(errmsg, "use_reg is provided an illegeal value: %d, \
                            only 0 or 1 can be provided!", use_reg);
            printDebugInfo(errmsg);
            exit(0);
        }
    }

    if( ads5294_cfg->decimation_opt != -1 ){
        config_ads5294_decimation( ads5294_cfg, ads5294_cfg->decimation_opt );
    }

    return used_reg_num;
}

//  print ads5294 configurations for debug
int print_ads5294_config( ADS5294_Config *cfg ){
    printf("##################################\n");
    printf("#        ads5294 registers        #\n");
    printf("> reg_addr : reg_data(hex) reg_data(dec) reg_flag\n");

    int idx;
    for( idx=0; idx<cfg->reg_num; idx++ ){
        printf("> 0X%02X     : 0X%02X\t %d\t %d\n", 
                cfg->reg_addr[idx],
                cfg->reg_data[idx],
                cfg->reg_data[idx],
                cfg->reg_flag[idx] );
    }
    
    printf("##################################\n");
    
    return idx;
}

//  update configuration for a given reg_addr with value reg_data
int update_ads5294_config( ADS5294_Config *cfg, uint32_t reg_addr, uint32_t reg_data ){

    int i, cnt=0;
    int found = -1;

    for( i=0; i<cfg->reg_num; i++ ){
        if( reg_addr == cfg->reg_addr[i] ){
            cfg->reg_data[i] = reg_data;
            cfg->reg_flag[i] = 1;   // 既然已经更新了，就是要配置的，所以别忘了这个设置
            found = 1;
            break;
        }

        cnt++;
    }

    if( found != 1 ){
        char errmsg[1024];
        sprintf(errmsg, "failed updating register: 0X%02X", reg_addr);
        printDebugInfo(errmsg);
    }

    return 0;
}

///////////////////////////////////////////////////////////////////////
// SPI data format for ads5294:
//  A7|A6|...|A1|A0|D15|D14|...|D1|D0
uint32_t config_ads5294_via_SPI( uint32_t *gpio_reg, 
					             uint32_t *gpio2_reg, 
					             uint32_t *gpio_in_reg,
					             SPI_CONFIG *spi_config,
					             ADS5294_Config *cfg
					            ){
    int idx, used_reg_num = 0;
    for( idx=0; idx<cfg->reg_num; idx++ ){
        used_reg_num += cfg->reg_flag[idx];
    }

    if( used_reg_num == 0 ){
        printf("> (config_ads5294_via_SPI): used_reg_num = 0, no data needs to be transferred via SPI.\n");
    }
    
    // prepare spi_data    
    SPI_DATA *spi_data = (SPI_DATA*)malloc(sizeof(SPI_DATA));
    spi_data_alloc(spi_data, used_reg_num);
    
    uint32_t reg_cnt = 0;
    for( idx=0; idx<cfg->reg_num; idx++ ){

        if( cfg->reg_flag[idx] == 1 ){
            spi_data_set_LS( spi_data, 
                             reg_cnt, 
                             cfg->reg_addr[idx],
                             cfg->reg_data[idx],
                             16 );      // left shift by 16bits

            // for debug
            // if(cfg->reg_addr[idx] == 0x46){
            //     printf_bin(cfg->reg_data[idx]);
            //     exit(0);
            // }

            reg_cnt++;
            
            usleep(1000);
        }
    }
    
    if( reg_cnt != used_reg_num ){
        char msg[1024];
        sprintf(msg,"error happened when preparing spi_data, reg_cnt = %d, used_reg_num = %d",
                    reg_cnt, used_reg_num);
        printDebugInfo(msg);
    }
    
    // send SPI data to ads5294
    send_spi_data(  gpio_reg,
                    gpio2_reg,
                    gpio_in_reg,
                    spi_config, 
                    spi_data );
                    
    // finish SPI comm., free allocated memory
    spi_data_free(spi_data);
    
    return 0;
}


//  对一些常用的控制功能做函数封装：
void power_on_ads5294(  uint32_t *gpio_reg,
                        uint32_t *gpio2_reg,
                        SPI_CONFIG *spi_config,
                        ADS5294_Config *cfg ){
//  NOT implemented yet!
}

void power_off_ads5294( uint32_t *gpio_reg,
                        uint32_t *gpio2_reg,
                        SPI_CONFIG *spi_config,
                        ADS5294_Config *cfg ){
//  NOT implemented yet!
}

//  根据预设的滤波设置更新对应的寄存器
int config_ads5294_decimation( ADS5294_Config *cfg, int opt ){

    // check opt, it can only be 2 or 4
    if( (opt != 2) & (opt != 4) ){
        char errmsg[1024];
        sprintf(errmsg, "opt can only be 2 or 4, but you provided %d\n", opt);
        printDebugInfo(errmsg);
        printf("> (ads5294) skip configuring decimation registers ...\n");
        return 0;
    }

    uint32_t reg_data;
    uint32_t bits_D9D8D7;
    uint32_t bits_D6D5D4;
    uint32_t bits_D2;
    uint32_t bits_D0;

    if( opt == 2 ){
        printf("> (ads5294) updating filter registers for preset decimation by 2 (low-pass odd-tap, 0--fs/4)\n");

        // 0X29
        reg_data = 0b0000000000000010;
        update_ads5294_config(cfg, 0x29, reg_data);


        bits_D9D8D7 = 0b000;    // FILTERx_COEFF_SET
        bits_D6D5D4 = 0b000;    // FILTERx_RATE
        bits_D2     = 0b1;      // OOD_TAPx
        bits_D0     = 0b1;      // USE_FILTER_CHx

        reg_data = 0;
        reg_data |= bits_D9D8D7 << 7;
        reg_data |= bits_D6D5D4 << 4;
        reg_data |= bits_D2 << 2;
        reg_data |= bits_D0 << 0;

        // 0X2E
        update_ads5294_config(cfg, 0x2E, reg_data);
        
        // 0X2F
        update_ads5294_config(cfg, 0x2F, reg_data);

        // 0X30
        update_ads5294_config(cfg, 0x30, reg_data);

        // 0X31
        update_ads5294_config(cfg, 0x31, reg_data);

        // 0X32
        update_ads5294_config(cfg, 0x32, reg_data);

        // 0X33
        update_ads5294_config(cfg, 0x33, reg_data);

        // 0X34
        update_ads5294_config(cfg, 0x34, reg_data);

        // 0X35
        update_ads5294_config(cfg, 0x35, reg_data);

        // 0X38
        reg_data = 0b01;    // fs/2
        update_ads5294_config(cfg, 0x38, reg_data);

        // 0XD1
        reg_data = 0X0240;  // decimation by 2
        update_ads5294_config(cfg, 0xD1, reg_data);
    }

    if( opt == 4 ){
        printf("> (ads5294) updating filter registers for preset decimation by 4 (low-pass odd-tap, 0--fs/8)\n");

        // 0X29
        reg_data = 0b0000000000000010;
        update_ads5294_config(cfg, 0x29, reg_data);

        bits_D9D8D7 = 0b010;    // FILTERx_COEFF_SET
        bits_D6D5D4 = 0b001;    // FILTERx_RATE
        bits_D2     = 0b0;      // OOD_TAPx
        bits_D0     = 0b1;      // USE_FILTER_CHx

        reg_data = 0;
        reg_data |= bits_D9D8D7 << 7;
        reg_data |= bits_D6D5D4 << 4;
        reg_data |= bits_D2 << 2;
        reg_data |= bits_D0 << 0;

        // 0X2E
        update_ads5294_config(cfg, 0x2E, reg_data);
        
        // 0X2F
        update_ads5294_config(cfg, 0x2F, reg_data);

        // 0X30
        update_ads5294_config(cfg, 0x30, reg_data);

        // 0X31
        update_ads5294_config(cfg, 0x31, reg_data);

        // 0X32
        update_ads5294_config(cfg, 0x32, reg_data);

        // 0X33
        update_ads5294_config(cfg, 0x33, reg_data);

        // 0X34
        update_ads5294_config(cfg, 0x34, reg_data);

        // 0X35
        update_ads5294_config(cfg, 0x35, reg_data);

        // 0X38
        reg_data = 0b10;    // fs/4
        update_ads5294_config(cfg, 0x38, reg_data);

        // 0XD1
        reg_data = 0X0140;  // decimation by 4
        update_ads5294_config(cfg, 0xD1, reg_data);
        printf("> (ads5294) configured register 0XD1.\n");
    }

    printf("> (ads5294) finished updating filter registers ...\n");

    return 0;
}
